#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorH,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_2,     motorI,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C4_1,     motorH,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorI,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void initializeRobot()
{
  // servo[servo3] = 59;
  // servo[servo4] = ;
  // servo[servo5] = 80;
}
#include "JoystickDriver.c"
//#include "Recorder1.c"
#include "Recorder2.c"
task main()
{
  int x;
  int y;
  nMotorEncoder[motorD] = 0;
  nMotorEncoder[motorE] = 0;
  int Z = 10;
  while(true)                            // Infinite loop:
  {
   // waitForStart();
    getJoystickSettings(joystick);

    if(abs(joystick.joy1_y2) > Z)   // If the right analog stick's Y-axis readings are either above or below the Q:
    {
      motor[motorE] = ((joystick.joy1_y2));         // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the Q:
    {
      motor[motorE] = 0;                        // Motor D is stopped with a power level of 0.
    }


    if(abs(joystick.joy1_y1) > Z)   // If the left analog stick's Y-axis readings are either above or below the Q:
    {
      motor[motorD] = ((joystick.joy1_y1));         // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
    }
    else                                    // Else if the readings are within the Q:
    {
      motor[motorD] = 0;                        // Motor E is stopped with a power level of 0.
    }

    if(joy2Btn(6))
  {
    motor[motorG] = -100;
  }
 if(joy2Btn(5))
  {
    motor[motorG] = 0;
  }

  if(joy2Btn(1))
  {
      servo[servo1] = 243;
      servo[servo2] = 0;
  }
  if(joy2Btn(2))
  {
      servo[servo1] = 140;
      servo[servo2] = 110;
   }
if(joy2Btn(3))
  {
    servo[servo3] = 38;
    servo[servo4] = 255;
    servo[servo5] = 103;
  }
if(joy2Btn(4))
{
 motor[motorH] = 100;
}
else
{
  motor[motorH] = 0;
}


  if(joy2Btn(8) || joy2Btn(7))
  {
    if(joy2Btn(8))
      motor[motorF] = 100;
    if(joy2Btn(7))
      motor[motorF] = -100;
  }
  else
 {
    motor[motorF] = 0;
 }
 }
}
